/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.ortn.wildbots.commands;
import edu.ortn.wildbots.subsystems.Launcher;
import edu.wpi.first.wpilibj.DriverStationLCD;
import java.util.Enumeration;
import java.util.Vector;
/**
 *
 * @author Stephen
 */
public class LauncherControl extends CommandBase{
    
    public static int FIRE = 1;
    public static int SPEED_UP = 2;
    public static int STOP = 3;
    private boolean isMotorRunning;
        
    private Vector comQueue = new Vector();
    
    public LauncherControl(){
        requires(launcher);
        
    }
    protected void initialize() {
        launcher.reset();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        int command = ((Integer)comQueue.firstElement()).intValue();
        if (command == FIRE) {
            launcher.fire();
        }
        if (command == SPEED_UP) {
            launcher.setLauncherEnabled(true);
            isMotorRunning = true;
        }
        if (command == STOP) {
            launcher.stopMotor();
            isMotorRunning = false;
        }
        comQueue.removeElementAt(0);
        launcher.update();
        
        DriverStationLCD lcd = DriverStationLCD.getInstance();
        String[] data = launcher.getData();
        lcd.println(DriverStationLCD.Line.kMain6, 1, data[0]);
        lcd.println(DriverStationLCD.Line.kUser2, 1, data[1]);
        lcd.updateLCD();
    }

    // Called once after isFinished returns true
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
        launcher.reset();
    }
    
    protected void interrupted() {
    }
    
    public void fire() {
        comQueue.addElement(new Integer(FIRE));
    }
    
    public void startMotor() {
        if (!isMotorRunning)
            comQueue.addElement(new Integer(SPEED_UP));
    }
    
    public void stopMotor() {
        if (isMotorRunning)
            comQueue.addElement(new Integer(STOP));
    }
    
}
